#pragma config(Sensor, in1,    rightsensor,    sensorLineFollower)
#pragma config(Sensor, in2,    middlesensor,   sensorLineFollower)
#pragma config(Sensor, in3,    leftsensor,     sensorLineFollower)
#pragma config(Sensor, in8,    pot,            sensorPotentiometer)
#pragma config(Sensor, dgtl1,  frontLimit,     sensorTouch)
#pragma config(Sensor, dgtl2,  backLimit,      sensorTouch)
#pragma config(Sensor, dgtl10, ultrasonic,     sensorSONAR_cm)
#pragma config(Motor,  port1,           left,          tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port6,           claw,          tmotorVex269, openLoop)
#pragma config(Motor,  port7,           arm,           tmotorVex393, openLoop)
#pragma config(Motor,  port10,          right,         tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

void Ultradistance (int stopDistance);
void turning_point (int degrees, bool direction);
void forward ( float inches );
void turning_swing (int degrees, bool direction);
void arm_control (int setPoint);
void follow_wall ();
void lineFollow ();
task main()
{
	while (SensorValue[ultrasonic]<     )
	{
		lineFollow;
	}
	motor[right]=0;
	motor[left



}

void follow_wall ()
{
	if (SensorValue[frontLimit]==1 && SensorValue[backLimit]==1)
		{
			motor[left]=80;
			motor[right]=80;
		}
	else if (SensorValue[frontLimit]==0 && SensorValue[backLimit]==1)
		{
			motor[left]=80;
			motor[right]=40;
		}
	else if (SensorValue[frontLimit]==1 && SensorValue[backLimit]==0)
		{
			motor[left]=-40;
			motor[right]=0;
		}
	else if (SensorValue[frontLimit]==0 && SensorValue[backLimit]==0)
		{
			motor[left]=60;
			motor[right]=30;
		}
}

void arm_control (int setPoint)
{
	motor[arm]= .5*(setPoint - SensorValue[pot]);
}
void lineFollow ()
{
	if (SensorValue[middlesensor]>1000)
		{
			motor[right]=60;
		  motor[left]=60;
		}
		else
		{
			if (SensorValue[leftsensor]>1500)
			{
				motor[right]=75;
				motor[left]=-100;
			}
			else
			{
				motor[right]=-100;
				motor[left]=75;
			}
		}
}


void forward ( float inches )
{
	int milliseconds = 0;

	milliseconds=(inches-4.1328)/0.0255;
	motor[left]=127;
	motor[right]=122;
	wait1Msec(milliseconds);
	motor[left]=0;
	motor[right]=0;
}
void turning_swing (int degrees, bool direction)
{  //1 is left 0 is right dinna work
	int milliseconds=0;
	milliseconds=(40*degrees)/3;
	if(direction==1)
	{
		motor[right]=96;
		wait1Msec(milliseconds);
		motor[right]=0;
	}
	else
	{
		motor[left]=96;
		wait1Msec(milliseconds);
		motor[left]=0;
	}
}
void Ultradistance (int stopDistance)
{
	while (SensorValue[ultrasonic] > stopDistance && SensorValue[ultrasonic]!=-1)
	{
		motor[left]=127;
		motor[right]=127;
	}
	motor[left]=0;
	motor[right]=0;
}

void turning_point (int degrees, bool direction)
{  //right is one, left is 0
	int milliseconds=0;
	milliseconds=degrees/0.144;
	if(direction==1)
	{
		motor[right]=-94;
		motor[left]=96;
		wait1Msec(milliseconds);
		motor[right]=0;
		motor[left]=0;
	}
	else
	{
		motor[right]=94;
		motor[left]=-96;
		wait1Msec(milliseconds);
		motor[right]=0;
		motor[left]=0;
	}
}
